/***************************************************************************
 创建者: 林逢达
 开始时间: 2016.11.19
 copyright: (C) 同川科技
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)

 ***************************************************************************
 *
 *                                                                         *
 ***************************************************************************/
#ifndef CHAINSOLVERTEST_HPP
#define CHAINSOLVERTEST_HPP

#include"chainModels.hpp"
#include<kdl/chain.hpp>
#include<kdl/chainfksolverpos_recursive.hpp>
#include<kdl/chainfksolvervel_recursive.hpp>
#include<kdl/chainidsolver_recursive_newton_euler.hpp>
#include<kdl/chainidsolver_vereshchagin.hpp>
#include<kdl/chainiksolverpos_lma.hpp>
#include<kdl/chainiksolverpos_nr.hpp>
#include<kdl/chainiksolverpos_nr_jl.hpp>
#include<kdl/chainiksolvervel_pinv.hpp>
#include<kdl/chainiksolvervel_pinv_givens.hpp>
#include<kdl/chainiksolvervel_pinv_nso.hpp>
#include<kdl/chainiksolvervel_wdls.hpp>

using namespace KDL;

class ChainSolverTest
{
public:
    ChainSolverTest();
    void chainFkPosSolverTest();
    void chainIkPosSolverTest_LMA();
    void chainIkPosSolverTest_NR();
    void chainIkPosSolverTest_NR_JL();
    void chainFkandIkVelTest();
    void chainIkVelTest_nso_wdls();
    void chainFkandIkAccTest();
    void chainIdSolverTest();
    void printTimeNano(struct timespec& t1,struct timespec& t2);
    ~ChainSolverTest();

private:
    Chain puma;
    Chain kuka;
    int jnPuma;
    int jnKuka;
    JntArray jointPuma;
    JntArray jointKuka;
    void chainIkPosSolver(const JntArray& joint, ChainFkSolverPos& fkPosSolver, JntArray& jointInit, ChainIkSolverPos& ikPosSolver, JntArray& jointOut);
    void chainVelSolver(const Chain& chain,  ChainFkSolverVel& fkSolverVel, ChainIkSolverVel& ikSolverVel);
    void chainAccSolver(const Chain& chain, ChainFkSolverAcc& fkSolverAcc, ChainIkSolverAcc& ikSolverAcc);

};


#endif // CHAINSOLVERTEST_HPP
